// Copyright 2017-2018 Filmstorm Pty Ltd - All Rights Reserved.
#include "MotionMatchingHelpers.h"
#include "Components/SkeletalMeshComponent.h"
#include "Engine.h"
#include "AnimationRuntime.h"
#include "MM.h"

//The driving time of the animation data utilities
const float DeltaTime = 0.1f;

//Animation velocity derived at a specific time
void UMotionMatchingHelpers::GetAnimVelocityReferenceClock(const UAnimSequence * InSequence, const float ReferenceClock, FVector& DeliveredVelocityViaAnimBonesLocalVelocity)
{
	//Our temporary velocity to help with root motion extraction
	FVector TmpVelo = InSequence->ExtractRootMotion(ReferenceClock, DeltaTime, true).GetTranslation();
	//we found the velocity now what do we do with it?? We set it from the temp velocity against the size / time.
	DeliveredVelocityViaAnimBonesLocalVelocity = TmpVelo.GetSafeNormal() * (TmpVelo.Size() / DeltaTime);
	//return
	return;
}
//Our animation data utilities are helpign extract our smoothed rotation from our delivered rotation, using our clock in game and animation sequence.
void UMotionMatchingHelpers::ExtractAnimSmoothedRotation(FQuat & DeliveredRotation, const UAnimSequence * InSequence, const float ClockInGame)
{
	//first we start with an array of floats - let's call it weights
	TArray <float> Weights;
	//secondly we'll set another array to handle the rotations
	TArray <FTransform> Rots;
	//Okay now will set a time variable and clamp to ensure we are getting the values we expect
	const float Time = FMath::Clamp(ClockInGame - .1f, 0.f, InSequence->SequenceLength);

	//Now for i - we are getting ten rotations and adding them for the extract root motion method.
	for (int32 i = 0; i < 10; i++)
	{
		Rots.Add(InSequence->ExtractRootMotion(Time, 1.f * i, true));
		Weights.Add(0.1f);
	}
	//Our calculated Transform
	FTransform CalculatedTransformMM;
	//From our runtime we are now blending the transform by weight
	FAnimationRuntime::BlendTransformsByWeight(CalculatedTransformMM, Rots, Weights);
	//the delivered Rotation from the calculated transform.
	DeliveredRotation = CalculatedTransformMM.GetRotation();
}
//What we really need now is to extract the animation input direction.
void UMotionMatchingHelpers::ExtractAnimInputDirection(FInputPlayerDirectionData& OutInputDirectionData, const UAnimSequence * InSequence, const float ClockInGame)
{
	//If we aren't in any sequences right now we should go back to motherbase
	if (!InSequence)
	{
		//exit
		return;
	}
	//Here's our root prediction data again from the blueprint (our desired input direction)
	OutInputDirectionData = FInputPlayerDirectionData();
	//End time
	float EndTime = ClockInGame + 1.f;
	//Middle Time
	float MidTime = ClockInGame + 0.5f;
	//Create a transform and call it KeyTransform
	FTransform KeyTransform;
	//We'll get our bone transform and set it to the key transform and the clock in game.
	InSequence->GetBoneTransform(KeyTransform, 0, ClockInGame, false);
	//new array to hold our input direction points.
	TArray <FPlayerDirectionInput> DirectionalPointsArray;
	//Again let's reference ten points for the system to manage
	for (int i = 0; i < 10; i++)
	{
		//New  float for time delay
		float TimeDelay = (0.1 * i);
		//our input direction point transform, which is new, to hold the extracted root motion
		FTransform InputDirectionPointTransform = InSequence->ExtractRootMotion(ClockInGame, TimeDelay, true);
		//add the points to oour input direction point holder.
		DirectionalPointsArray.Add(FPlayerDirectionInput(InputDirectionPointTransform, TimeDelay));
	}
	//We've now set the data to our array
	OutInputDirectionData.DirectionalPointsArray = DirectionalPointsArray;
	GetAnimVelocityReferenceClock(InSequence, ClockInGame + 1.f, OutInputDirectionData.FinishVector);
}
//Get the animation bones local transform from the current skeleton part we are reference and the calculated transform point.
void UMotionMatchingHelpers::GetAnimBoneLocalTransform(const UAnimSequence * InSequence, const float ReferenceClock, const int CurrentSkeletonPartInReference, FTransform & CalculatedTransformMM)
{
	CalculatedTransformMM = FTransform::Identity;

	bool bCheckAnimOutput = false;

	if (InSequence && (CurrentSkeletonPartInReference != INDEX_NONE))
	{
		InSequence->GetBoneTransform(CalculatedTransformMM, CurrentSkeletonPartInReference, ReferenceClock, bCheckAnimOutput);

		USkeleton* SourceSkeleton = InSequence->GetSkeleton();
		//Our reference skeleton
		FReferenceSkeleton RefSkel = SourceSkeleton->GetReferenceSkeleton();
		//make sure the skeleton is in a valid reference
		if (RefSkel.IsValidIndex(CurrentSkeletonPartInReference))
		{
			int SelectedAnimation = CurrentSkeletonPartInReference;

			if (SelectedAnimation == 0)
			{
				InSequence->GetBoneTransform(CalculatedTransformMM, SelectedAnimation, ReferenceClock, bCheckAnimOutput);
				return;
			}
			//The reference skeletons parent index is not zero
			while (RefSkel.GetParentIndex(SelectedAnimation) != 0)
			{
				//set an integer of the selected animation
				int ParentIndex = RefSkel.GetParentIndex(SelectedAnimation);
				//our parent transform
				FTransform ParentTransform;
				InSequence->GetBoneTransform(ParentTransform, ParentIndex, ReferenceClock, bCheckAnimOutput);
				//set the parent transform
				CalculatedTransformMM = CalculatedTransformMM * ParentTransform;
				SelectedAnimation = ParentIndex;
			}
		}
		else
		{
			//zero out the calculated transform if we finish
			CalculatedTransformMM = FTransform::Identity;
		}
	}
}
//Grab the world transform and again reference the part of the skeleton and calculated transform
void UMotionMatchingHelpers::GetAnimBoneWorldTransform(const UAnimSequence * InSequence, const float ReferenceClock, const int CurrentSkeletonPartInReference, FTransform & CalculatedTransformMM)
{
	//we should set this at zero
	CalculatedTransformMM = FTransform::Identity;
	//for sanity check this at false
	bool bCheckAnimOutput = false;
	//if our sequence and our index...
	if (InSequence && (CurrentSkeletonPartInReference != INDEX_NONE))
	{
		//get the bone transform of the calculated transform and the current skeleton reference
		InSequence->GetBoneTransform(CalculatedTransformMM, CurrentSkeletonPartInReference, ReferenceClock, bCheckAnimOutput);
		//our source skeleton
		USkeleton* SourceSkeleton = InSequence->GetSkeleton();
		//set our reference skeleton to our source skeleton
		FReferenceSkeleton RefSkel = SourceSkeleton->GetReferenceSkeleton();
		//if our reference skeleton is valid
		if (RefSkel.IsValidIndex(CurrentSkeletonPartInReference))
		{
			//our selected animation
			int SelectedAnimation = CurrentSkeletonPartInReference;
			//if our selected animation is the first one
			if (SelectedAnimation == 0)
			{
				//perform get the bone transform
				InSequence->GetBoneTransform(CalculatedTransformMM, SelectedAnimation, ReferenceClock, bCheckAnimOutput);
				return;
			}
			//While the skeleton animation isn't in index
			while (RefSkel.GetParentIndex(SelectedAnimation) != INDEX_NONE)
			{
				//create a new integer and set it to the current selected animation index
				int ParentIndex = RefSkel.GetParentIndex(SelectedAnimation);
				//create a new transform to hold the parent transform
				FTransform ParentTransform;
				//Our parent transform getter bone transform
				InSequence->GetBoneTransform(ParentTransform, ParentIndex, ReferenceClock, bCheckAnimOutput);
				//Our calculated transform motion matched towards the parent transform
				CalculatedTransformMM = CalculatedTransformMM * ParentTransform;
				//Our selected animation
				SelectedAnimation = ParentIndex;
			}
		}
		else
		{
			//refresh the calculated transform if no data
			CalculatedTransformMM = FTransform::Identity;
		}
	}
}
//Let's get our animation bones local velocity and get the bone we are referencing and push it to the delivered velocity via animation bones local velocity.
void UMotionMatchingHelpers::GetAnimBoneLocalVel(const UAnimSequence * InSequence, const float ReferenceClock, const int CurrentSkeletonPartInReference, FVector & DeliveredVelocityViaAnimBonesLocalVel)
{
	if (InSequence)
	{
		//zero the new bone transform for good measure
		FTransform BoneTransform = FTransform::Identity;

		GetAnimBoneLocalTransform(InSequence, ReferenceClock, CurrentSkeletonPartInReference, BoneTransform);
		FTransform PastBoneTransform = FTransform::Identity;

		//Calculating the anim bone local transform - against the time - delta time passing through the skeleton reference and the past bone transforms.
		GetAnimBoneLocalTransform(InSequence, ReferenceClock - DeltaTime, CurrentSkeletonPartInReference, PastBoneTransform);

		FVector TmpVelo = BoneTransform.GetLocation() - PastBoneTransform.GetLocation();
		float DirectionInputStrength = TmpVelo.Size() / DeltaTime;
		FVector AppliedVelocity = TmpVelo.GetSafeNormal() * DirectionInputStrength;
		//Well here it is, the delivered velocity
		DeliveredVelocityViaAnimBonesLocalVel = AppliedVelocity;
	}
}

//Our animation joint data - more referencing to the time, bone names and actual skeleton references
void UMotionMatchingHelpers::GetAnimJointData(const UAnimSequence * InSequence, const float ReferenceClock, const FName BoneName, FSkeletonStructure & DeliveredAnimationReferencesForSkelRef)
{
	DeliveredAnimationReferencesForSkelRef = FSkeletonStructure();
	if (InSequence && (BoneName != NAME_None))
	{
		//Get our source skeleton
		USkeleton* SourceSkeleton = InSequence->GetSkeleton();
		//set our reference skeleton to match the source skeleton
		FReferenceSkeleton RefSkel = SourceSkeleton->GetReferenceSkeleton();
		//our integer for our bone index
		const int CurrentSkeletonPartInReference = RefSkel.FindBoneIndex(BoneName);
		//our bool for the local velocity
		bool LocalVel = false;
		//If our local velocity ...
		if (LocalVel)
		{
			//Create a transform for the bone transform and set it to zero for sanity
			FTransform BoneTransform = FTransform::Identity;
			//Get animation bone local transform and get the current part reference and bone transform
			GetAnimBoneLocalTransform(InSequence, ReferenceClock, CurrentSkeletonPartInReference, BoneTransform);
			//Create a past bone transform and zero it
			FTransform PastBoneTransform = FTransform::Identity;
			//calculate the local transform from the past bone transform
			GetAnimBoneLocalTransform(InSequence, ReferenceClock - DeltaTime, CurrentSkeletonPartInReference, PastBoneTransform);
			//Our temp velocity - take the current location and minus it against the past bone transform
			FVector TmpVelo = BoneTransform.GetLocation() - PastBoneTransform.GetLocation();
			//Our float to hold the direction input strength
			float DirectionInputStrength = TmpVelo.Size() / DeltaTime;
			//the applied velocity - get the normal and multiply it against the direction input strength.
			FVector AppliedVelocity = TmpVelo.GetSafeNormal() * DirectionInputStrength;
			//Our bone transforms positions
			DeliveredAnimationReferencesForSkelRef.BoneTransformPosition = BoneTransform.GetLocation();
			//Our bone transofms velocity
			DeliveredAnimationReferencesForSkelRef.BoneTransformVelocity = AppliedVelocity;
		}
		else
		{
			//new transform for root transform
			FTransform RootTransform;
			//push in the sequence / Get the bone transform agains the time from the reference clock.
			InSequence->GetBoneTransform(RootTransform, 0, ReferenceClock, false);

			//Zero out the transform
			FTransform BoneTransform = FTransform::Identity;
			//Find the bones world transform against the time from our reference clock.
			GetAnimBoneWorldTransform(InSequence, ReferenceClock, CurrentSkeletonPartInReference, BoneTransform);

			//Zero out the transform --- handle the past bone transform
			/*
			02/12/2016 ---- Note this was originally not applied however after playtesting we need to reference the past bones to ensure
			stability of the controller and playback of the motion match results.
			*/
			FTransform PastBoneTransform = FTransform::Identity;
			GetAnimBoneWorldTransform(InSequence, ReferenceClock - DeltaTime, CurrentSkeletonPartInReference, PastBoneTransform);

			//Temporary Velocity - gathered from bone transform location and past bone transforms.
			FVector TmpVelo = BoneTransform.GetLocation() - PastBoneTransform.GetLocation();
			//Get Direction Input strenght from the temporary velocity over time.
			float DirectionInputStrength = TmpVelo.Size() / DeltaTime;
			//Our final data for Bone Position
			DeliveredAnimationReferencesForSkelRef.BoneTransformPosition = RootTransform.InverseTransformPositionNoScale(BoneTransform.GetLocation());
			//Our final data for Velocity Position
			DeliveredAnimationReferencesForSkelRef.BoneTransformVelocity = RootTransform.InverseTransformVectorNoScale(TmpVelo.GetSafeNormal()) * DirectionInputStrength;
		}
	}
}

//Let's get key pose data from the animation system - save it into the calculated motion matching data
void UMotionMatchingHelpers::GetKeyPoseDataFromAnim(const UAnimSequence * InSequence, const float ReferenceClock, const TArray<FName> ToBeRefAgainst, FMotionMatchData& CalculatedMMData)
{
	if (InSequence)
	{
		//Create our referenced skeleton - we need to get data from it
		FReferenceSkeleton RefSkel = InSequence->GetSkeleton()->GetReferenceSkeleton();
		//Put our key pose data into our new variable
		CalculatedMMData = FMotionMatchData();
		//Our retarget source is equal to our sequence source
		CalculatedMMData.ElectedSourceReference = InSequence->RetargetSource;
		//for the amount of the references we are holding
		for (int i = 0; i < ToBeRefAgainst.Num(); i++)
		{
			//Our bone transforms will equal zero
			FTransform BoneTransform = FTransform::Identity;
			//set to false for sanity
			bool bCheckAnimOutput = false;
			//Get our bone transform from the sequence and get our bone index.
			InSequence->GetBoneTransform(BoneTransform, RefSkel.FindBoneIndex(ToBeRefAgainst[i]), ReferenceClock, bCheckAnimOutput);
			//Add our bones transforms into our calculated Motion Matching data.
			CalculatedMMData.SelectedSkeletonBoneTransforms.Add(BoneTransform);
		}
	}
}
/*
Where it all comes together - time to analyze the provided animation, let's get the skeleton runtime name and our present input direction,
our future input direction also the array of our delivered animation data we so nicely bundled together.
*/
void UMotionMatchingHelpers::GetDataFromAnimation(const UAnimSequence * InSequence, const float ReferenceClock, const TArray<FName> SkeletonRuntimeName, FInputPlayerDirectionData & OutPresentInputDirection, FInputPlayerDirectionData & OutFutureInputDirection, TArray<FSkeletonStructure>& DeliveredAnimationReferencesForSkelRef)
{
	//If we are in a sequence right now - that's a good start
	if (InSequence)
	{
		//for the amount of skeleton runtime names we are currently referencing let's check...
		for (int i = 0; i < SkeletonRuntimeName.Num(); i++)
		{
			//our joint data - create a new reference
			FSkeletonStructure NewJointData = FSkeletonStructure();
			//and get our data from the array
			UMotionMatchingHelpers::GetAnimJointData(InSequence, ReferenceClock, SkeletonRuntimeName[i], NewJointData);
			//put that into the array under the new joint data
			DeliveredAnimationReferencesForSkelRef.Add(NewJointData);
		}
		//We will now extract the anim input via the present input direction
		UMotionMatchingHelpers::ExtractAnimInputDirection(OutPresentInputDirection, InSequence, ReferenceClock - 1.f);
		//Also the future input direction
		UMotionMatchingHelpers::ExtractAnimInputDirection(OutFutureInputDirection, InSequence, ReferenceClock);
	}
}
//Time to compare our calculated trajectories - we are using the input direction from our blueprint
float UMotionMatchingHelpers::CompareTrajectories(const FInputPlayerDirectionData InputDirectionData_A, const FInputPlayerDirectionData InputDirectionData_B)
{
	//Our Transform Data fed into the Root Prediction Data
	FInputPlayerDirectionData TD = InputDirectionData_A;
	//Return the comparison as well
	return TD.CompareTo(InputDirectionData_B, 4);
}
//Compare the joint data  - run it through the array
float UMotionMatchingHelpers::CompareJointDatas(const TArray<FSkeletonStructure> JointData_A, const TArray<FSkeletonStructure> JointData_B)
{
	//If our joint data A greater than nil and A and B equal the same number
	if ((JointData_A.Num() > 0) && (JointData_A.Num() == JointData_B.Num()))
	{
		//our float for holding the pose cost
		float PoseCost = 0.f;
		//for our joint data count
		for (int i = 0; i < JointData_A.Num(); i++)
		{
			//our pose cost plus the compared joint datas count
			PoseCost += JointData_A[i].CompareTo(JointData_B[i]);
		}
		//our pose cost
		//PoseCost;
		//return the pose to motherbase
		return PoseCost;
	}
	//else return a generic pose cost value
	return -666.f;
}
/*
Animation data utilities for handling creation of our input direction data from the vectors we have left behind
also the taking into account input direction and the checking transform array.
*/
bool UMotionMatchingHelpers::MakeInputDirectionData(FInputPlayerDirectionData& OutInputDirection, const TArray<FTransform> VectorsWeHaveLeftBehind, const TArray<FVector> CheckTheseAgainstVWHLB)
{
	if (VectorsWeHaveLeftBehind.Num() == 11)
	{
		//create a transfor the past transform
		FTransform PastTransform = VectorsWeHaveLeftBehind[0];
		//our input direction to hold the root prediction data
		OutInputDirection = FInputPlayerDirectionData();
		//for the amount of vectors remaining...
		for (int i = 0; i < VectorsWeHaveLeftBehind.Num(); i++)
		{
			//get the input direction points and add them to the direction point.
			OutInputDirection.DirectionalPointsArray.Add(FPlayerDirectionInput(VectorsWeHaveLeftBehind[i].GetRelativeTransform(PastTransform), 0.1f * i));
		}
		//Put four vectors that the character has moved past into the transform for other difference
		FTransform TransformForOtherDifference = VectorsWeHaveLeftBehind[4];
		TransformForOtherDifference.Blend(VectorsWeHaveLeftBehind[4], VectorsWeHaveLeftBehind[5], 0.5f);
		//return true if it all worked correctly
		return true;
	}
	//oops - we should go back now.
	return false;
}
//Draw the input direction for data to pertain some visual reference.
void UMotionMatchingHelpers::DrawInputDirectionData(const FInputPlayerDirectionData InputDirection, const FTransform& WorldTransform, FPrimitiveDrawInterface * PDI, const FColor InColor, const uint8 Depth)
{
	//Thickness of our line
	float InputDirectionThickness = 2.f;
	//Generate some points for our data to grip onto
	for (int i = 1; i < InputDirection.DirectionalPointsArray.Num(); i++)
	{
		FVector Curr = WorldTransform.TransformPosition(InputDirection.DirectionalPointsArray[i].CurrentRefTransform.GetTranslation());
		FVector Past = WorldTransform.TransformPosition(InputDirection.DirectionalPointsArray[i - 1].CurrentRefTransform.GetTranslation());
		//Smash a line out
		PDI->DrawLine(Curr, Past, InColor, Depth, InputDirectionThickness);
	}
}
//Blend those key poses - Pose A and Pose B against the float of alpha our submit our final pose to outblendedkeypose
void UMotionMatchingHelpers::BlendKeyPoses(const FMotionMatchData PoseA, const FMotionMatchData PoseB, const float Alpha, FMotionMatchData & OutBlendedKeyPose)
{
	//If our pose are above zero
	if ((PoseA.SelectedSkeletonBoneTransforms.Num() > 0) && (PoseB.SelectedSkeletonBoneTransforms.Num() > 0))
	{
		//and our poses equal the same amount
		if (PoseA.SelectedSkeletonBoneTransforms.Num() == PoseB.SelectedSkeletonBoneTransforms.Num())
		{
			//our blended key pose will equal our first pose.
			OutBlendedKeyPose = PoseA;
			//calculate the total bone transforms
			for (int i = 0; i < PoseA.SelectedSkeletonBoneTransforms.Num(); i++)
			{
				//Our bone transforms should blend against our alpha float again.
				OutBlendedKeyPose.SelectedSkeletonBoneTransforms[i].Blend(PoseA.SelectedSkeletonBoneTransforms[i], PoseB.SelectedSkeletonBoneTransforms[i], Alpha);
			}
		}
	}
}
//We should also construct a target direction to help with our input system
void UMotionMatchingHelpers::CreateInputDirection(FInputPlayerDirectionData & OutConstructTargetDirection, const FTransform& DesiredTransform, const FTransform& RootWorldTransform, const float TargetDirectionInputStrength)
{
	OutConstructTargetDirection = FInputPlayerDirectionData();
	//need a new transform to hold the current transform
	FTransform CurrentTransform;

	for (int i = 0; i < 10; i++)
	{
		float dataCalc = ((float)(i + 1)) / ((float)10);
		//let's blend these transforms togethers
		CurrentTransform.Blend(RootWorldTransform, DesiredTransform, dataCalc);
		//hold a new point transform
		FTransform PointTransform = CurrentTransform;
		//ALRIGHT!! Let's finally construct the target direction
		OutConstructTargetDirection.DirectionalPointsArray.Add(FPlayerDirectionInput(PointTransform.GetRelativeTransform(RootWorldTransform), 0.1f * i));
	}
	//let's make the true desired direction agains the inverse transform of the last current transforms rotation (if that makes sense) and the forward vector of said rotation.
	const FVector TrueDesDir = RootWorldTransform.InverseTransformVectorNoScale(CurrentTransform.GetRotation().GetForwardVector());
	//Fianlly output that target.
	OutConstructTargetDirection.FinishVector = TrueDesDir * TargetDirectionInputStrength;
}

//Handle all that good AnimationReferences
FMotionMatchingMath::FMotionMatchingMath()
{
	//The actual animation source
	SrcAnimIndex = INDEX_NONE;
	//The start time - wow.
	StartTime = 0.f;
	//The crucial node for our blueprint calculation to designate the root predication data in the struct.
	PlayersPredictedInputData = FInputPlayerDirectionData();
	//Okay our motion joint data
	MotionJointData.Empty();
}

//Our extracted data from the animation - grabs the source animation at the correct time and references that again the skeleton name.
void FMotionMatchingMath::ExtractDataFromAnimation(const UAnimSequence * InSequence, const int AtSrcAnimIndex, const float AtSrcStartTime, TArray <FName> SkeletonRuntimeName)
{
	if (InSequence)
	{
		// our animation source
		SrcAnimIndex = AtSrcAnimIndex;
		//The skeleton we are referencing
		FReferenceSkeleton RefSkel = InSequence->GetSkeleton()->GetReferenceSkeleton();
		//the start time.
		StartTime = AtSrcStartTime;

		for (int i = 0; i < SkeletonRuntimeName.Num(); i++)
		{
			//Our joint data
			FSkeletonStructure NewJointData = FSkeletonStructure();
			UMotionMatchingHelpers::GetAnimJointData(InSequence, StartTime, SkeletonRuntimeName[i], NewJointData);
			MotionJointData.Add(NewJointData);
		}
		//Grab anim velocity
		UMotionMatchingHelpers::GetAnimVelocityReferenceClock(InSequence, StartTime, PresentVel);
		UMotionMatchingHelpers::ExtractAnimInputDirection(PlayersPredictedInputData, InSequence, StartTime);
	}
}
//Calculate the cost of the motion matching via our input reaction speed, velocity strength, animation match power against the input direction, present joint data and current tracking velocity. (what a mouth full)
float FMotionMatchingMath::AnimationMatchMathData(const float ControllerInputReactionSpeed, const float VelocityStrength, const float AnimationMatchPower, const FInputPlayerDirectionData InputDirection, const TArray <FSkeletonStructure> PresentJointData, const FVector CurrentTrackingVelocity)
{
	//A float to hold the cost
	float Cost = 0.f;
	//Grab the distance of the present velocity and the current tracking velocity and times it by the velocity strength
	Cost += FVector::Dist(PresentVel, CurrentTrackingVelocity) * VelocityStrength;
	/*Our inputdirection is then compared to the future direction and times against the controller input reaction speed
	---
	Fun note: this is super important for the responsiveness of the players controller, the ControllerInputReactionSpeed
	should always be adjusted to a value above 5 to provide a decent playback.
	The values usually range at:
	5 - for walking
	10 - for jogging
	30 - for sprinting
	---
	*/
	Cost += InputDirection.CompareTo(PlayersPredictedInputData, 1) * ControllerInputReactionSpeed;
	//Run a check
	check(PresentJointData.Num() == MotionJointData.Num());
	//Hold the pose cost
	float PoseCost = 0.f;
	//present joint data with the index
	for (int i = 0; i < PresentJointData.Num(); i++)
	{
		PoseCost += PresentJointData[i].CompareTo(MotionJointData[i]);
	}

	PoseCost *= AnimationMatchPower;
	//return the pose cost and the cost of the input direction and velocity distance for analysis.
	return PoseCost + Cost;
}

//Animation Analyzer configure setup
UAnimationAnalyzer::UAnimationAnalyzer(const FObjectInitializer& ObjectInitializer)
	:Super(ObjectInitializer)
{
	//Time step set at 0.1 for smoother analysis
	WorldClock = 0.1f;
	//Make sure the skeleton begins as null
	SelectedSkeleton = NULL;
	//clear anim data
	AnimationReferences.Empty();
	//clear motion bones
	ReferencedJoints.Empty();
}

//Get the lowest cost of the animation data to correctly play what we want to playh
int UAnimationAnalyzer::StructureForAnimationCalculations
(
	//controller input reaction speed - how sensitive the character is to movement
	const float ControllerInputReactionSpeed,
	//our velocity strength - how far forward can we push the stick for calculations
	const float VelocityStrength,
	//how hard the animations will try to match each other
	const float AnimationMatchPower,
	//Our calculated input direction from the blueprint
	const FInputPlayerDirectionData PlayerInputDirection,
	//Find our joint data from the animation analyzer
	const TArray <FSkeletonStructure> CurrentUserSelectedBones,
	//Our current velocity that is present
	const FVector CurrentVelocity,
	//Finally our lowest cost
	float & CalculatedResult
)
{
	//Best target to present to the system
	int BestResultingMatch = INDEX_NONE;
	//Lowest cost available
	float MinimumAllowance = 9999999999.f;

	if (AnimationReferences.Num() > 0)
	{
		for (int i = 0; i < AnimationReferences.Num(); i++)
		{
			//Again we are computing the current cost of the animation data via our input variables
			float CalcBase = AnimationReferences[i].AnimationMatchMathData(ControllerInputReactionSpeed, VelocityStrength, AnimationMatchPower, PlayerInputDirection, CurrentUserSelectedBones, CurrentVelocity);
			//Check to see if the current cost is less than the lowest cost
			if (CalcBase < MinimumAllowance)
			{
				//Okay we found the optimal target and are setting a variable for it
				BestResultingMatch = i;
				//assume control of the our new target
				MinimumAllowance = CalcBase;
			}
		}
	}
	CalculatedResult = MinimumAllowance;
	//Send the optimal target back to motherbase for analysis
	return BestResultingMatch;
}

//Clear all the animations from memory
void UAnimationAnalyzer::CleanAnimationReferences(const int AtSourceAnimIndex)
{
	//Check if the animation data is greater than zero, if it is let's remove the animations
	if (AnimationReferences.Num() > 0)
	{
		for (int i = AnimationReferences.Num() - 1; i > -1; i--)
		{
			if (AnimationReferences[i].SrcAnimIndex == AtSourceAnimIndex)
			{
				AnimationReferences.RemoveAt(i);
			}
		}
	}
}
//Run the clearer
void UAnimationAnalyzer::CleanAllAnimationReferences()
{
	Modify();
	AnimationReferences.Empty();
	MarkPackageDirty();
}
//Make sure the clearer ran
void UAnimationAnalyzer::AnalyzerAnimationReferences(const int AtSourceAnimIndex)
{
	Modify();

	CleanAnimationReferences(AtSourceAnimIndex);

	MarkPackageDirty();
}
//Run through the current animations selected in the system and make sure they are up to date- important
void UAnimationAnalyzer::ConstructAnimationReferences(const int  FromSourceAnimation)
{
	if (AnimationsInMemory[FromSourceAnimation])
	{
		Modify();

		AnalyzerAnimationReferences(FromSourceAnimation);

		const float AnimationTimeLength = AnimationsInMemory[FromSourceAnimation]->GetPlayLength();
		float TimeForCalc = 0.f;
		//Possible optimization in the future - 18/01/16 logged in internal memo
		while (TimeForCalc <= AnimationTimeLength)
		{
			bool CanKey = (TimeForCalc >= 1.f) && (TimeForCalc <= (AnimationTimeLength - 1.f));

			if (CanKey)
			{
				FMotionMatchingMath NewMotionMatchingMath = FMotionMatchingMath();
				NewMotionMatchingMath.ExtractDataFromAnimation(AnimationsInMemory[FromSourceAnimation], FromSourceAnimation, TimeForCalc, ReferencedJoints);
				AnimationReferences.Add(NewMotionMatchingMath);
			}

			TimeForCalc += WorldClock;
		}

		MarkPackageDirty();
	}
}
//Super clean, destroy all data and rebake from start
void UAnimationAnalyzer::ConstructAllAnimationReferences()
{
	CleanAllAnimationReferences();
	if (AnimationsInMemory.Num() > 0)
	{
		for (int32 i = 0; i < AnimationsInMemory.Num(); i++)
		{
			ConstructAnimationReferences(i);
		}
	}
}

//Add an animation to the system
void UAnimationAnalyzer::NewAnimationReference(UAnimSequence * CreateNewAnimationReference)
{
	Modify();
	AnimationsInMemory.Add(CreateNewAnimationReference);
	ConstructAllAnimationReferences();
	MarkPackageDirty();
}
//Remove a source animation
void UAnimationAnalyzer::RemoveAnimationReference(const int AnimationReference)
{
	Modify();

	AnimationsInMemory.RemoveAt(AnimationReference);

	ConstructAllAnimationReferences();

	MarkPackageDirty();
}
//Ensure this is the correct frame to reference
bool UAnimationAnalyzer::IsAnimationExtracted(const FName AnimationReference, const float WorldTime)
{
	if (AnimationReferences.Num() > 0)
	{
		for (int i = 0; i < AnimationReferences.Num(); i++)
		{
			if (AnimationReferences[i].StartTime == WorldTime)
			{
				return true;
			}
		}
	}
	return false;
}